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1  SUMMARY 


The  meaning  of  Space  Situational  Awareness  (SSA),  which  has  drawn  a  lot  of  attention  recently, 
is  more  than  just  the  characterization  of  the  space  environment  and  how  it  will  affect  space 
activities  [1,2].  SSA  activities  can  be  divided  into  many  possible  categories,  including  but  not 
limited  to  detecting  new  space  objects  and  tracking  their  motion,  understanding  how  the  space 
environment  will  impact  mission  behaviors,  and  determining  useful  characteristics  of  identified 
objects  [3].  Interest  has  also  been  expressed  in  using  SSA  to  properly  track  nearby  or  distant 
objects,  while  being  aware  of  unexpected  rendezvous  by  outside  actors  [3]. 

As  space  becomes  more  and  more  crowded,  to  achieve  an  effective  SSA,  not  only  the  capabilities 
of  detecting  and  tracking  objects,  but  also  the  capabilities  of  rapid  response  and  re -planning 
semi-autonomously  or  autonomously  are  crucial.  The  following  are  just  a  few  planning  problems 
that  are  often  seen  for  rapid  SSA  response:  fonnation  keeping,  fonnation  reconfiguration, 
rendezvous  and  docking,  space  inspection  and  assembly  without  intensive  labor,  coordinated 
movement,  debris  avoidance,  and  proximity  operations  considering  exhaust  plume  impingement. 

Central  to  all  these  planning  activities  is  the  capability  of  computing  optimal  control  commands 
rapidly  while  considering  nonlinear  dynamics,  state  and  control  variable  constraints, 
environment  limitations,  etc.  The  objectives  may  be  the  minimum  fuel  maneuvers  needed  to 
achieve  a  maximized  life  time,  or  the  minimum  time  maneuvers  needed  to  achieve  a  quick 
response. 

In  the  perfonned  research,  the  virtual  motion  camouflage  (VMC)  based  optimal  planning 
algorithm,  inspired  by  the  observation  in  mating  hoverflies,  is  further  investigated  and  the 
performance  is  enhanced  significantly  in  terms  of  the  convergence  speed  and  collision  avoidance 
capability.  In  addition,  the  method  is  compared  with  the  newly  developed  local  pursuit  based 
method.  Also  the  dimension  and  time  complexities  of  the  innovative  algorithms  are  analyzed. 

The  algorithm  is  validated  in  an  enhanced  low  cost  robots  testbed. 
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2  INTRODUCTION 


To  date,  the  majority  of  local  optimal  trajectory  planning  methods  belongs  to  one  of  the 
following  categories:  mathematical  programming,  heuristic  approaches,  and  hybrid  methods.  It 
is  worth  noting  that  there  are  many  feasible,  real-time  methods,  such  as  the  A*  [4]  and  the 
rapidly  exploring  random  tree  approaches  [5].  The  calculus  of  variations  (CoV)  together  with 
the  Pontryagin’s  Minimum  Principle  (PMP)  approaches  and  the  Direct  Collocation  (DC)  with 
nonlinear  programming  (NLP)  approaches  are  two  main  methods  in  the  mathematic 
programming  category  [6] [7],  The  CoV  +  PMP  approach  works  effectively  when  there  is  no 
severe  state  inequality  constraint  and  a  good  initial  guess  is  provided.  DC  +  NLP  approach  can 
easily  incorporate  state  and  control  constraints,  and  its  initial  guess  is  not  that  difficult  to  obtain. 
However,  the  problem  dimension  of  the  achieved  NLP  is  normally  large  and  results  in  a  high 
computational  cost.  To  obtain  global  optimal  solutions,  many  heuristics  and  meta-heuristics 
based  methods  have  been  studied,  such  as  evolutionary  programming  [8]  and  particle  swanning 
[9],  As  mentioned  in  [10],  although  these  methods  work  effectively  for  many  different  problems, 
they  cannot  be  rigorously  proved  and  are  mostly  used  off-line  due  to  the  high  computational  cost. 

The  studied  virtual  motion  camouflage  (VMC)  and  local  pursuit  (LP)  techniques  belong  to  the 
hybrid  category,  in  which  the  trajectory  planning  problem  is  optimized  using  a  mathematical 
programming  approach  in  a  simultaneously  refined  manifold.  Specifically,  in  this  approach,  the 
path  of  a  vehicle  is  optimized  in  a  varying  subspace  or  manifold,  which  is  defined  by  a  virtual 
prey  motion  and  possibly  a  reference  point.  The  optimization  in  such  a  manifold  is  controlled  by 
a  single  dimension  parameter.  The  problem  dimension  in  the  achieved  NLP  is  small,  particularly 
for  the  cases  with  severe  constraints  and  many  obstacles.  However,  just  like  any  other  NLP  based 
approaches,  a  good  initial  guess  is  crucial  for  these  methods  to  converge  rapidly,  especially  when 
the  vehicle  is  required  to  navigate  in  an  obstacle-laden  environment. 

The  objectives  of  the  research  project  are: 

0 1 :  The  virtual  motion  camouflage  method,  inspired  by  the  observations  in  mating  hoverflies, 
was  further  investigated  for  rapid  optimal  planning  problems. 

02:  Detailed  analyses  about  the  enhanced  algorithm  were  conducted  and  the  performances, 

such  as  problem  dimension  and  time  complexity,  have  been  investigated.  Furthermore 
the  feasibility  and  flexibility  of  the  algorithm  were  studied  for  the  cases  when  the  number 
of  obstacles  and  the  number  of  vehicles  in  the  system  are  changing. 

and 

03:  These  algorithms  were  tested  in  a  significantly  enhanced  low  cost  robot  testbed. 
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3  METHODS,  ASSUMPTIONS,  AND  PROCEDURES 


The  research  conducted  is  summarized  here:  virtual  motion  camouflage  method,  initial  guess 
enhancement,  local  pursuit  method,  and  software  update. 

3.1  Virtual  Motion  Camouflage  Method 

A  male  hoverfly  (Fig.  1)  uses  the  motion  camouflage  to  fly  along  a  path  connecting  the  female 
hoverfly  and  a  background  reference  point  to  conceal  its  motion  from  the  view  point  of  the 
female  one  [11]. 


Figure  1.  Motion  camouflage  used  by  hoverflies 

An  inherent  thread  is  observed:  “the  predator  only  moves  along  paths  in  a  certain  manifold.  ” 
Inspired  by  this  observation,  the  simple  rule  used  by  the  hoverflies  can  be  mimicked  to  construct 
varying  subspaces  (or  “manifold”)  such  that  the  planning  problem  can  be  solved  rapidly.  In  the 
meantime,  to  avoid  potential  shortcomings  (i.e.  the  solution  optimality  depends  on  the  selection 
of  the  subspace),  the  actual  vehicle  will  follow  paths  that  are  optimized  in  an  iteratively  refined 
“manifold” . 

Expanding  on  these  results,  the  algorithm  is  outlined  in  the  following  steps: 

Step  1:  Dimension  reduction  via  the  motion  strategy.  The  simple  rule  in  the  virtual  motion 
strategy  is:  the  “position”  state  x  is  confined  by  the  motion  of  the  virtual  prey  xp ,  the  reference 

point  xr ,  and  the  path  control  parameter  (PCP)  v(t) .  The  original  problem  will  be  converted  into 
the  following  reduced  dimension  problem:  Given  xp ,  (possibly)  the  parameters  xr,  v(t),  i /(f), 

. . .,  will  be  designed  to  minimize  the  perfonnance  index  J2  =  J2{xr,v,\$-...tf)  subject  to  inequality 
constraints  (I.E.Cs.)  and  equality  constraints  (E.Cs).  It  is  worth  noting  that  the  optimization  of 
the  virtual  prey  motion  xp  will  be  conducted  in  Step  4  to  achieve  the  optimal  solution. 

Step  2:  Convert  the  problem  achieved  in  Step  1  into  a  nonlinear  programming  problem  (NLP) 
via  collocation.  Through  this  discretization,  the  perfonnance  index  can  be  re-written  as 
J3  =  J3(xr,v,tf) ,  in  which  v  =  [vQ,...,vN]T  is  the  discretized  version  of  the  PCP  v(t).  The 
derivatives  of  the  PCP  variables  can  be  found  from  dnv  /  dtn  =  [2  /  (tf  -  t0)]r  Dnv ,  in  which  D  is 
the  differentiation  matrix. 
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Step  3:  Further  dimension  reduction  via  the  necessary  conditions  derived  from  boundary 
conditions:  Certain  PCPs,  such  as  the  initial  PCP  ( v0 )  and  final  PCP  ( vN  ),  can  be  calculated 

instead  of  being  optimized.  It  is  worth  noting  that  the  necessary  conditions  are  different  for 
different  dynamical  systems  and  different  boundary  conditions. 

Step  4:  The  curve  of  the  virtual  prey  motion  xp  needs  be  flexible  enough  so  that  the  solution 

achieved  using  the  proposed  method  can  be  optimal.  In  the  meantime  the  number  of  the 
parameters  controlling  the  prey  motion  must  be  small  enough  such  that  the  computational  cost 
will  not  be  compromised  much.  In  this  step,  the  virtual  prey  motion  will  be  represented  using  13- 
spline  curves  [12]  and  the  control  points  P  for  the  B-spine  will  be  optimized.  Therefore  the  final 
achieved  problem  is:  the  performance  index  can  be  re-written  as  J4  =  J4(xr,v,P,tf) .  The  I.E.C.s. 
and E.C.s.  are  converted  to  g4(xr,v,P,tf)<  0  and  h4(xr,v,P,tf)  =  0,  respectively. 

3.2  Enhanced  Initial  Guess 

In  the  previous  version  of  the  VMC  algorithm,  a  straight  line  connecting  the  starting  and  ending 
points  is  used  as  the  initial  virtual  prey  path.  For  most  of  the  obstacle  avoidance  cases,  the 
algorithm  works  well  if  the  reference  point  in  the  VMC  algorithm  is  not  on  this  straight  line. 

However,  when  the  straight  line  of  the  virtual  prey  motion  happens  to  pass  through  the  center  of 
any  obstacle,  as  shown  in  Fig.2,  it  becomes  challenging  for  the  VMC  algorithm  (or  any  other 
gradient  based  methods)  to  rapidly  find  an  optimal  or  even  feasible  solution.  Therefore,  in  order 
to  improve  the  robustness  of  the  VMC  method,  an  alternative  approach  is  investigated  in 
milestone  1  to  obtain  a  good  initial  virtual  prey  motion  guess. 


Final  Position 


Figure  2.  A  “bad”  initial  guess  of  the  virtual  prey  path 

Figure  3  outlines  the  basic  steps  involved  in  finding  a  good  initial  guess  for  the  VMC  algorithm 
in  finding  nonlinear  optimal  trajectory  in  obstacle-laden  environment. 

Step  1:  The  wavefront  algorithm  [13]  is  used  to  find  an  obstacle-free  corridor,  and  the  virtual 
prey  path  in  this  corridor  will  generate  a  trajectory  with  a  high  possibility  of  being  obstacle- 
collision  free. 
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Step  2:  Since  the  path  generated  by  the  wavefront  method  doesn’t  involve  time  information  and 
may  not  be  smooth,  the  result  achieved  will  be  regarded  as  an  initial  guess  and  further  optimized 
in  a  B-spline  based  optimization.  An  arbitrary  perfonnance  index,  such  as  the  shortest  distance, 
can  be  used  in  the  optimization.  The  obstacle  avoidance  is  regarded  as  the  inequality  constraints. 

Step  3:  The  virtual  prey  found  in  Step  2  will  be  used  as  the  initial  guess  in  the  VMC  method. 

Empirically,  the  initial  guess  of  the  PCP  variables  should  be  1 .  With  the  enhanced  initial  guess 
strategy,  the  initial  selection  of  a  reference  point  is  not  as  sensitive  as  the  original  VMC  method. 
It  is  more  effective  if  the  reference  point  is  placed  far  away  from  the  actual  vehicle’s  path. 


Figure  3.  VMC  algorithm  with  enhanced  initial  guess  in  obstacle-laden  environments 
3.3  Local  Pursuit  Method 

The  local  pursuit  (LP)  motion  strategy  is  a  biological  phenomenon  found  in  the  behavior  patterns 
of  ants.  As  shown  in  Fig.  4,  an  ant  points  its  velocity  towards  the  position  of  the  ant  ahead  of  it  to 
achieve  the  minimum  time  performance. 


Figure  4.  LP  strategy 


The  LP  strategy  can  be  described  as 


ifl(0  =  v[x^(t-A)-xfl(0]  (1) 

in  which  the  position  of  prey  xp  is  A  steps  ahead  of  the  predator,  v  is  a  scale  variable  that  we 

call  the  path  control  parameter  (PCP)  so  as  to  be  consistent  with  the  VMC  method. 
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Since  the  prey  is  a  virtual  one,  the  value  of  A  will  not  affect  the  solution  optimality  and  thus  will 
be  regarded  as  zero.  Following  the  LP  motion  strategy  Eq.  (1),  the  position  vector  of  the  Lego 
robot,  x ,  is  formulated  by  the  trajectory  of  the  virtual  prey,  xp  □  [x  ,  yp  f ,  and  PCP,  v,  as 


x(0  +  vx(t)  =  VXp  it) 

The  acceleration  derivatives  can  be  derived  from  Eq.  (2),  as 

*(0  =  v(x p  (t)  -  xit))  +  v(xp  it)  -  xit)) 

Then,  the  control  variables  V  and  co  are  computed  via  the  dynamic  inversion  as 

jx  /  cos  (<9) ,  cos  (#)  *  0 
| y!  sin(^),  cos(^)  =  0 


(2) 

(3) 

(4) 


and 


co  =  {yx-xy)/[x2  +y2),  (x2+y2)^0  (5) 

respectively.  Using  a  high  order  discretization  method,  such  as  the  Legendre-Gauss-Lobatto 
(LGL)  method  [14],  the  position  of  Lego  robot  can  be  further  represented  as 

C^iD'+E)-'ECpJ  (6) 

in  which  Cv  =  tv]  ,  where  /  —  1,2  is  the  jth  direction  of  the  position  of  Lego  robot, 

subscript  0 denotes  the  discretization  node.  C p  j  -  [xp  f  xpJtN~\  is  the  discretized 
version  of  the  virtual  prey  position.  H  =  diag{v}  is  the  diagonal  matrix  with  a  proper  dimension 
and  v  is  the  discretized  version  of  the  SCP  variable.  D  is  the  differentiation  matrix  and 
D'u[2/(tf-t0)]D. 

Similarly,  the  discretized  velocity  and  acceleration  states  of  the  predator  can  be  derived  as 

f  .=(£)'  +  =)-'  [3  -  S  0  +  S)-'S]C„.  +(£>'  +  a)'1  SCrJ  (7) 


and 
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Cj  =  (D'  +  H)-1 


(8) 


+2{D  +  H)_1 


2=(D'  +  H)-[ 

fiefs' 


=  -a(D'+ =r'=]j'" 

f„+(D'+S)-'Sf„. 


To  make  the  virtual  prey  motion  flexible  and  enhance  the  quality  of  the  optimization  space,  13- 
spline,  which  is  a  method  using  a  small  number  of  control  points  to  define  a  flexible  curve,  is 
chosen  to  represent  the  virtual  prey  motion. 

For  example,  the  fh  direction  of  the  virtual  prey  “position”  at  node  k,  xp  .  k,j  =  1,2, A:  =  0,...,  N, 
can  be  represented  by  a  nonlinear  rational  B-spline  (NURBS)  [12]  curve  of  degree  d  ,  as 

xp,j,k  =Ze,A',)P,j,  j  =  1.2.  O) 

1=0 

where  Bid(t),i  = 0,...,ncp  are  the  d,h  degree  basis  functions,  P.,.,/ =  are  the  control 

points  for  the  jth  direction  of  the  virtual  prey  “position”,  and  ncp  + 1  is  the  number  of  control 

points.  The  B-spline  representation  of  the  virtual  prey  motion  Eq.  (9)  can  also  be  written  in  the 
vector  form  as 


CpJ=BPj,  j  =  l,...,na  (10) 

in  which  B  =  [Bid(tk)\,  i  =  0,...,ncp,  k  -  0,...,  TV ,  and  Pj  =[Pj0,...Pjn]  is  the  column  vector  of 
the  control  points. 

The  Ith  derivative  of  x  ,  can  be  written  either  in  the  scalar  form  as 

P,J,k 


dt' 


LZA‘>)p,~’J= i,2,k=o, 

m= 0 


N 


(11) 


or  in  the  matrix  form  as 


,  y  =  1,2  (12) 

ill  which  B1  h  =  |  /i.  '.  ( )  and  the  superscript  (/)  represents  the  l'h  derivative. 

To  further  reduce  the  number  of  optimizable  parameters  and  mitigate  the  difficulty  of  the 
convergence  when  equality  constraints  are  involved,  necessary  conditions  are  derived  based  on 
the  boundary  conditions  (BC),  which  are  used  to  calculate  a  part  of  the  control  points  and  PCPs. 
The  following  BCs  are  known:  the  initial  and  final  positions,  and  the  initial  velocity. 
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Thus,  after  given  the  first  PCP,  v0 ,  the  first  control  point  of  the  virtual  prey  can  be  calculated  by 


P„o  =  Xp,i, 0  =  0  +  K,u 0  /  v0 ,  y  =  1.2  (13) 

and  the  robot’s  position  at  all  the  discretized  nodes,  except  the  first  and  last  nodes,  will  be 
calculated  using, 


(14) 


in  which  MIP  □  ( D  +  E) ,  and  M LP , ,  M LP  N+l ,  and  M°p  are  the  first,  last,  and  remaining  columns 
of  M LP ,  respectively.  Ca j  0 ,  a .  v ,  and  ,  are  respectively  the  first,  last,  and  remaining 

“position  state”  variables  of  the  vehicle  in  the  i‘h  direction.  (  )  denotes  the  pseudo-inverse 
operation. 

Since  the  BC  of  the  Lego  robot  has  been  included  in  Eq.  (13)  and  Eq.  (14)  and  the  dynamic 
model  has  been  considered  in  Eq.  (4)  and  Eq.  (5),  there  is  no  equality  constraint  in  the  following 
formulated  nonlinear  programming  (NLP)  problem. 

N 

The  performance  index  J  =  0.5  [tf  -  f0)^  w;  is  optimized  subject  to  the  inequality  constraints 

1=0 

g(v',P',tf)<  0  ,  which  includes  the  limits  on  the  state  and  control  variables  and  obstacle 
avoidance.  Here  v  and  P'  are  the  subsets  of  the  parameters  in  v  and  P  that  are  optimized. 

For  convenience,  let’s  define  Set  SC)  to  include  all  the  parameters  to  be  optimized,  and  Set  Sc  to 

include  the  parameters,  which  are  calculated  using  the  necessary  conditions  derived.  The  steps 
involved  in  the  optimization  are  summarized  in  Algorithm  1 . 


Algorithm  1,  LP  Optimization  Algorithm 

Steps  in  the 
Initialization 

Step  1: 

Provide  initial  guesses  for  the  parameters  in  SC) . 

Step  2; 

Calculate  the  parameters  in  Sc  using  the  appropriate 
necessary  condition  described. 

Step  3; 

Construct  the  virtual  prey  motion  using  B-splines. 

Step  4 

Compute  the  state  and  control  variables. 

Step  5; 

Evaluate  the  perfonnance  index. 

Steps  inside 
the  NLP 

Step  6; 

Evaluate  the  inequality  constraints. 

If  the  convergence  criterion  is  not  satisfied  and  the 
maximum  number  of  iterations  has  not  been  reached, 

Step  7; 

update  the  parameters  in  Set  S0  for  the  next  iteration,  and 

go  back  to  Step  2.  Otherwise,  the  optimization  is 
terminated. 
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3.4  Software  and  Graphical  User  Interface  (GUI)  Updates 


The  LP  based  algorithm  is  programmed  and  packaged  in  the  Lego  robot  testbed  software 
(delivered  to  AFRL-RVSV). 

(1)  Subroutine:  BLP  TRAJ 

(2)  Objective:  Find  minimum-time  obstacle  avoidance  optimal  path  by  LP  method. 

N 

(3)  Equation:  min  J  =  0.5  (tf  -f,)X  vv< »  such  that  inequality  constraints  g(v',  P',tf)<  0  ,  v  is  a 

i=0 

scalar  and  P'  is  a  vector. 

(4)  Syntax 

[pos_true,time_true,PATHGEN_CPU,i_count,tf,error_flag]=eval(’BLP_TRAJ(struct_robot,i_sort 
ed_points,i_replan_iteration, figs, handles)'):  Returns  the  dimensionalized  optimal  path,  optimal 
time,  central  process  unit  (CPU)  calculation  time,  etc. 

(5)  Input  and  Output  Arguments  and  Function  Calls.  (Table  1) 


Table  ] 

l.  Input  and  Output  Arguments  and  Function  Calls 

Variable 

Variable  description 

Dimension 

structrobot 

Information  related  to  robot,  and  defined  in  the  main 
document 

i  sorted  points 

Pixel  locations  of  two  color  dots  on  the  Lego  robot 

1*4 

i  replan  iteration11 

The  number  of  planning  horizons 

1 

figs 

Defined  in  the  main  document 

handles 

Defined  in  the  main  document 

Output 

Variable 

Variable  description 

Dimension 

posture 

Optimal  position  (dimensionless)  of  the  Lego  robot 
in  the  test  bed 

2*(N+1) 

time  true 

Time  span  (dimensionless)  discretized  by  the  LGL 
method 

1*(N+1) 

pathgenCPU 

The  overall  CPU  time  to  generate  an  optimal 
trajectory  by  the  LP  method 

1 

i  count 

Number  of  tries 

1 

tf 

The  minimum  time  (in  dimension)  for  the  Lego 
robot  to  move  from  its  initial  position  to  its  final 
position 

1 

error  flag  (**) 

Whether  or  not  the  initial  guess  of  the  prey  path  is  a 
good  one 

1 
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4  RESULTS  AND  DISCUSSIONS 


4.1  Model  Used  in  Simulation  and  Hardware  Validation 

The  B-spline  augmented  virtual  motion  camouflage  (BVMC)  approach  is  customized  for  the 
two-wheel  driven  robot  with  a  specific  boundary  condition  (BC).  The  motion  of  the  robot  [15]  is 
governed  by  the  following  dynamics  model 


X 

cos  6 

~0~ 

y 

= 

sin  6 

V  + 

0 

6 

0 

1 

(15) 


in  which  ra  □  [x,  y]r  □  [ral,ra2]T  e  tR2  is  the  midpoint  position  of  the  robot,  and  6  is  the  heading 
angle.  The  position  of  the  robot  is  confined  within  the  test  area  as  xmjn  <  x  <  xmax  and 
ym <  y  <  ymax .  The  speed  V  and  the  turning  rate  (0  are  the  control  variables,  which  are 
constrained  by  \V\  <  Vmax  and  \co\  <  <umax ,  respectively.  The  parameters  associated  with  hardware 

limitations  are  found  from  experiments,  which  may  vary  depending  on  the  battery  level  onboard 
robots. 

The  randomly  distributed  nobs  obstacles  are  assumed  to  be  represented  by  circles,  thus  the 
midpoint  positions  of  the  robots  need  to  satisfy  ||r  -  rohs  i  ||  >  Robsi  +  abuf ,  i  =  1, ...,  nohs ,  where  robs  t 
is  the  center  of  the  ith  obstacles,  and  Robs  t  is  the  corresponding  radius.  ahuf  is  a  buffer  to  take 
into  account  the  maximum  width  of  the  robot  from  its  midpoint. 

The  planning  objective  of  the  robots  is  to  navigate  through  a  series  of  obstacles  from  their 
starting  position  to  chosen  ending  positions  with  a  minimized  final  time  J  =  dt ,  satisfying  the 

J‘o 

above  mentioned  constraints.  In  addition,  the  robots  are  required  to  re-plan  their  minimum  time 
trajectories,  while  taking  the  new  additional/pop-up  obstacles  into  account. 

The  position  vector  of  the  robot  ra  is  optimized  within  a  varying  subspace  (or  “manifold”), 
which  is  defined  by  two  main  parameters,  the  trajectory  of  the  virtual  prey 
rp  □  [xp,ypf  0[Vi’rA2f  and  a  selected  reference  point  rref  =[xref,yref]T  as 

ra  =rref+V(rp-rref)  (16) 

Here  a  scale  variable  called  the  path  control  parameter  (PCP)  v  determines  how  the  aggressor 
behaves  inside  this  varying  subspace  (or  “manifold”)  using  the  motion  camouflage  (MC)  rule 
(Eq.  2).  Its  velocity  and  acceleration  derivatives  are  defined  as 

ra  =  v(rp  -  rref)  +  vrp 
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(17) 


and 


fa  =  v{rp  -  rref )  +  vrp  +  2 vrp  ( 1 8) 

It  is  worth  noting  that  the  reference  point  is  constant  but  optimized.  If  V  ^  0 ,  the  heading  angle 
6  is  calculated  by  0  =  tan-1  ( y  /  x)  if  x  ^  0  or  6  =  sin-1  (y  IV)  if  x  =  0 .  The  speed  V  and  the 

heading  rate  co  can  be  calculated  using  V  =  sjx2  +  y2  and  co  =  (yx  -  xy)  /  V2 .  If  V  =  0 ,  6  can  be 
any  value,  and  the  heading  rate  can  be  zero. 

There  are  two  simultaneous  steps  involved  in  varying  the  robot  trajectory  in  the  problem  search 
space:  the  subspace  (or  “manifold”)  is  varied  and  the  actual  position  of  the  robot  is  varied  in  the 
subspace.  First  if  the  subspace  defined  by  rp  and  rref  is  fixed,  the  result  obtained  may  be  optimal 

only  in  the  subspace  but  not  optimal  in  the  space  defined  in  the  original  problem.  To  address  this 
issue,  the  parameters  that  define  the  subspace,  i.e.  the  reference  point  and  the  virtual  prey 
motion,  need  to  be  simultaneously  optimized  along  with  the  variation  of  the  robot  trajectory. 

The  reference  point  rref  can  be  included  in  the  later  achieved  nonlinear  program  problem  and 
optimized.  The  virtual  prey  motion  rp  needs  to  be  varied  carefully.  In  B-spline  augmented  VMC 
(BVMC)  method,  B-spline  curves  are  used  to  represent  the  virtual  prey  motion  trajectory  rp  as 

=  ''==!. 2  (19) 

k= 0 

where  n  + 1  is  the  number  of  the  control  points  P  =  [Pik\,  k  =  0 ,  i  =  1, 2 ,  detennining  the 
shape  and  direction  of  the  B-spline  curves.  Unlike  polynomials,  B-splines  have  good  local 
control,  which  allows  for  more  stable  curves.  Bk  d(t),  k  =  0,...,ncp  are  the  dth  degree  basis 
functions. 

Second,  the  position  of  the  robot  that  is  controlled  by  the  PCP  variable  within  the  subspace  (or 
“manifold”)  is  discretized  to  0 nodes  using  a  high  order  discretization  method  [14].  The 

discretized  PCP  vector  is  v  =  [v0,  vv...,  vNf  etR,v+1)xl,  andthe  k"'  derivative  of  the  PCP  vector  is 
found  with  the  equation 


dkv/dtk 


(20) 


in  which  D  is  the  differentiation  matrix  [14].  The  discretized  minimum  time  cost  function  is 
then  written  as 


N 

J  =  0.5  [tf  -  h) )  wi 

i= 0 


(21) 
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and  the  inequality  constraint  is 


g(v,P,rref,t)<  0,  g  e  9lpxl 


(22) 


which  includes  the  limits  on  the  state  and  control  variables,  obstacle  avoidance,  and  collision 
avoidance  among  the  robots  as  described  before.  The  equality  constraint  h(v,P,rref,t )  =  0, 

h  e  sJCxl ,  includes  the  BC.  The  dynamic  model  Eq.  (1)  has  already  been  considered,  thus  not 
included  as  the  equality  constraint  here. 

To  further  reduce  the  number  of  optimizable  parameters  in  the  achieved  NLP  and  mitigate  the 
difficulty  of  the  convergence,  necessary  conditions  are  derived  based  on  the  BC  and  used  to 
calculate  part  of  the  control  points  and  PCPs. 

4.2  Robustness  of  the  Enhanced  VMC  Method 

To  test  the  robustness  of  the  enhanced  VMC  algorithm,  many  simulation  cases  with  lots  of 
obstacles  are  conducted.  Only  six  cases  are  shown  in  the  following  pictures.  In  these 
simulations,  a  robot  navigating  through  a  test  area  with  randomly  generated  obstacles  is 
simulated.  The  cases  shown  are  just  six  out  of  the  1000-run  Monte  Carlo  simulation.  The  CPU 
time  is  less  than  4.72  seconds  when  programmed  in  Matrix  Laboratory  (MATLAB). 
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x-direction  (m) 


x-direction  (m)  x-direction  (m) 


4  6  8  2  4  6  8 

x-direction  (m)  x-direction  (m) 

Figure  5.  Simulation  runs  for  different  settings 


4.3  Sensitivity  Analysis  of  the  Enhanced  VMC  Method 


In  order  to  study  the  sensitivity  of  the  parameters  in  the  enhanced  VMC  method,  thousands  of 
Monte  Carlo  runs  are  simulated.  In  the  studies,  a  robot  is  commanded  to  move  from  one  corner 
of  the  testbed  to  the  other  areas  in  the  minimum  time,  while  avoiding  obstacles  that  are  randomly 
generalized.  The  following  tunable  parameters  are  varied:  (a)  the  number  of  control  points  of  the 
B-spline  curve,  (b)  the  degree  of  the  B-spline  curve,  (c)  the  number  and  the  radius  of  the 
obstacles,  (d)  the  initial  turning  angle  of  the  robot;  (e)  the  number  of  discretization  nodes;  and  (f) 
the  reference  point.  The  following  parameters  are  not  varying:  (a)  the  length  and  width  of  the 
testbed  (640  pixel  x  480  pixel),  (b)  the  initial  position  and  the  final  position  of  the  robot  is 
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uniformly  generated  in  the  corner  [0,  20;  0,  20]  pixel  location  and  [620,  640;  460,  480]  pixel 
location  of  the  testbed,  respectively;  (c)  500  Monte  Carlo  runs  are  performed  for  each  tunable 
parameter  case;  and  (d)  the  initial  PCPs  are  set  to  be  one. 

The  parameter  is  regarded  as  sensitive  if  the  change  of  this  parameter  will  produce  significant 
change  in  the  successful  rate.  For  example,  the  number  of  control  points  is  sensitive  as  shown  in 
Table  2.  When  the  number  of  control  points  is  5,  29  out  of  500  runs  fail;  while  if  the  number  of 
control  points  is  set  to  be  8,  only  7  out  of  500  runs  (1.4%)  fail. 

It  can  be  seen  from  the  following  table  that  only  the  degree  of  B-spline  is  less  sensitive. 
Therefore,  it  can  be  concluded  that  it  is  important  to  tune  the  parameters  properly  in  order  to 
achieve  very  robust  results. 


Table  2.  Sensitivity  Analysis 


Tunable 

Parameters 

Sensitive 
or  Not 

Examples 

Case  1 

#  Failed/500 

Case  2 

#  Failed/500 

#  of  Control 
Points 

Sensitive 

5 

29/500 

8 

7/500 

Degree  of  B- 
Spline 

Less 

sensitive 

4 

2/500 

6 

5/500 

#  of  Discre¬ 
tization 

Points 

Sensitive 

10 

20/500 

15 

6/500 

Reference 

Point 

Sensitive 

-[1800,1800]  pixel 

6/500 

-[18000,18000]  pixel 

39/500 

Initial  Angle 

Sensitive 

[0  90]° 

25/500 

[40  45]° 

6/500 

#  of 

Obstacles 

Sensitive 

[1  10] 

2/500 

[30  40] 

51/500 

Radius  of 
Obstacles 

Sensitive 

[5  15]  pixel 

6/500 

[20  30]  pixel 

125/500 

In  this  section,  ten  hardware  experiments  are  collected  and  compared.  All  the  programs  are 
written  in  the  Matlab  (R2010b)  with  the  following  toolboxes:  Acquisition,  Image  Processing, 
RWTH-Mindstorms  Nxt,  and  Optimization.  All  computations  were  performed  on  the  same 
laptop  with  a  frequency  of  2GHz  and  random  access  memory  (RAM)  of  6GB. 

4.4  Experiments  Comparison  between  VMC  and  LP 

Experiment  Settings: 

These  ten  experiments  are  categorized  in  two  groups.  In  the  first  group,  the  whole  optimization 
is  completed  in  one  planning  horizon,  while  in  the  second  group,  two  planning  horizons  are  used 
to  handle  the  unexpected  pop-up  obstacles. 
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In  each  test,  the  Lego  robot  will  move  from  a  randomly  generated  initial  position  to  a  randomly 
placed  final  position  using  both  the  LP  and  VMC  methods.  Four  or  live  obstacles  with  the  same 
radius  will  be  placed  randomly  on  the  test  bed.  The  maximum  translational  speed  of  the  Lego 
robot  is  measured  as  F  v  =  15  cm  /  5  ,  while  the  maximum  rotational  speed  is  v  =  16  rad  /  5  . 

indA  x  max 


Performance  Index  and  Success  Rate  Comparison: 

The  performance  index  (PI)  and  CPU  time  of  the  1st  group  are  shown  in  Table  3. 


Table  3.  The  performance  index  and  CPU  time  of  the  1st  group 


Group  1 

LP 

VMC 

Test  1 

CPU  Time 

3.69 

8.08 

PI 

29.24 

31.56 

Test  2 

CPU  Time 

2.16 

3.44 

PI 

30.29 

30.23 

Test  3 

CPU  Time 

4.81 

32.28 

PI 

38.41 

39.54 

Test  4 

CPU  Time 

4.72 

5.44 

PI 

39.22 

39.54 

Test  5 

CPU  Time 

3.11 

5.10 

PI 

36.17 

37.18 

Test  6 

CPU  Time 

5.03 

9.51 

PI 

33.13 

34.07 

For  each  test,  the  minimum  performance  index  among  these  two  methods  is  regarded  as  the 
“best”  solution.  If  the  performance  index  from  the  other  method  is  within  5%  difference  of  the 
best  solution,  the  solution  found  using  this  method  is  regarded  as  an  “optimal”  one;  otherwise  it 
will  be  regarded  as  a  “feasible”  solution  only. 


□  Best 

■  Optimal 

□  Feasible 


(a)  LP  Method 


(b)  VMC  Method 


Figure  6.  Use  Case  1  for  solutions  obtained  using  the  LP  method  and  the  VMC  method 

From  Table  2  and  Fig.  6,  the  following  observations  are  obvious:  (i)  Both  the  LP  and  VMC 
methods  can  help  Lego  robot  find  minimum-time  path  successfully,  the  largest  difference 
between  the  Pis  is  only  7%.  (ii)  The  LP  method  has  much  higher  “best”  solution  rate  in  finding 
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the  optimal  solutions  as  compared  with  that  of  the  VMC  method,  (iii)  The  CPU  time  needed  in 
the  VMC  method  is  in  the  range  of  3-9s,  while  the  LP  method  has  a  much  smaller  CPU  time, 
which  is  in  the  range  of  2-5s. 

It  is  worth  mentioning  that  to  record  the  experiment  results,  some  unnecessary  CPU  time  is 
wasted  in  exporting  some  information  during  the  optimization. 

The  performance  index  (PI)  and  CPU  time  of  the  2nd  groups  are  shown  in  Table  4  and  the 
comparison  of  solutions  is  shown  in  Fig.  7.  Observations  similar  to  the  first  group  can  be  made 
for  the  second  group.  It  is  worth  noting  that  the  performance  index  is  changed  during  the  two 
planning  horizons  because  the  distance  to  the  desired  position  is  reduced. 

Table  4.  The  performance  index  and  CPU  time  of  the  2nd  group 


Group  2 

LP 

VMC 

LP 

VMC 

1st  planning 

2nd  planning 

Test  1 

CPU  Time 

3.29 

9.23 

2.67 

8.42 

PI 

31.28 

31.83 

20.41 

22.05 

Test  2 

CPU  Time 

4.25 

6.33 

2.41 

4.15 

PI 

30.45 

29.79 

19.23 

18.39 

Test  3 

CPU  Time 

4.05 

6.35 

2.78 

6.81 

PI 

29.40 

30.78 

18.80 

20.64 

Test  4 

CPU  Time 

3.97 

6.45 

2.07 

4.69 

PI 

34.75 

36.13 

21.82 

22.38 

D  Best 
■  Optimal 
□  Feasible 


Figure  7.  Use  Case  2  for  solutions  obtained  using  the  LP  method  and  the  VMC  method 

Optimal  Trajectories: 

Two  tests  (Test  2  and  Test  4)  from  the  1st  group  are  selected  and  shown  in  Fig.  8  and  Fig.  9, 
respectively.  It  can  be  seen  from  Fig.  8  and  Table  2  that  the  optimal  trajectories  (represented  by 
the  red  lines)  and  the  minimum  time  to  arrive  at  the  desired  position  calculated  by  the  VMC 
method  and  the  LP  method  are  approximately  the  same,  and  Lego  robot  can  track  the  optimal 
trajectory  (represented  by  the  blue,  dotted  line)  very  well  using  the  low  level  tracking  controller. 
Seen  from  Fig.  9  and  Table  3,  the  optimal  trajectory  generated  by  the  LP  method  is  smoother 
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than  the  one  generated  by  the  VMC  method,  and  the  minimum  time  calculated  by  the  LP  method 
is  smaller  than  that  of  the  VMC  method. 


(a) 


(b) 


Figure  8.  The  optimal  trajectory  generated  in  test  2  of  the  1st  group:  (a)  VMC  method;  (b) 

LP  method 


(a)  (b) 


Figure  9.  The  optimal  trajectory  generated  in  test  4  of  the  1st  group:  (a)  VMC  method;  (b) 

LP  method 

One  result  from  the  2nd  group  is  shown  in  the  Fig.  10.  Obstacles  1-3  are  known  a  priori,  while 
obstacles  4-5  are  popped  up  obstacles.  As  shown  in  Fig.  6,  once  the  optimal  trajectories  are 
computed  using  either  the  VMC  method  or  the  LP  method,  the  Lego  robot  will  follow  the 
generated  path.  When  new  obstacles  appear,  the  Lego  robot  will  stop  and  wait  for  a  new  optimal 
trajectory  to  be  generated  and  then  follow  the  re-planned  trajectory  to  reach  the  desired  final 
position.  It  can  be  seen  that  the  planned  path  in  the  first  horizon  passes  through  obstacle  4 
(unknown),  while  the  re-planned  trajectory  corrects  the  trajectory  in  the  second  horizon. 
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(a) 


(b) 


Figure  10.  The  optimal  trajectories  generated  in  test  3  of  the  1st  group:  (a)  VMC  method 

(b)  LP  method 
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5  CONCLUSIONS 


In  this  research,  the  VMC  trajectory  optimization  method  is  enhanced.  The  performance  of  the 
VMC  method  is  compared  with  a  new  strategy  (LP)  based  method.  It  is  concluded  that  the  LP 
method  can  achieve  a  better  perfonnance  than  the  VMC  method  in  terms  of  convergence  rate, 
optimization  speed,  and  solution  optimality.  New  perturbation  methods  are  proposed  to  enhance 
the  initial  guess. 

The  following  publications  have  been  achieved: 

♦  N.,  Li,  Y.  Xu,  K.  Pham,  “Micro  Air  Vehicle’s  3D  Trajectory  Planning  and  Parametric 
Estimation,”  2013  AIAA  GNC,  Boston,  MA,  August  18-22,  2013. 

♦  A  journal  version  is  under  preparation. 

The  following  software  packages  have  been  delivered  to  AFRL-RV. 

♦  Software  Version  1.0  for  2D  Robot  Trajectory  Planning  in  Obstacle-Laden  Environment 
(In  report  1) 

♦  Software  Version  2.0  for  2D  Robot  Trajectory  Planning  in  Obstacle-Laden  Environment 
(Monte  Carlo  Simulation  Version)  (in  Report  2) 

♦  Software  Version  3.0  for  2D  Robot  Trajectory  Planning  in  Obstacle-Laden  Environment 
(Monte  Carlo  Simulation  Version)  (in  Report  3) 

♦  Ten  Experiment  Videos  are  submitted  (in  Report  3). 
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6  FUTURE  WORK 


Some  potential  research  and  development  improvements  include:  (i)  The  LP  method  needs  to  be 
further  enhanced  and  used  in  decentralized  cooperative  trajectory  planning  problems,  (ii)  System 
identification  or  estimation  methods  can  be  used  online  to  quantify  the  coefficients  of  the 
nonlinear  dynamic  model  and  a  receding  horizon  framework  can  be  used  to  update  the  command 
generation.  Stability  needs  to  be  proven  for  such  a  system,  (iii)  A  helicopter  platfonn  needs  to  be 
incorporated  into  the  testbed  to  provide  real-time  information  about  the  test  area  from  a  moving 
platform,  (iv)  Communication  issues  such  as  delay  is  one  of  the  next  steps  to  be  studied. 
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LIST  OF  ACRONYMS,  ABBREVIATIONS,  AND  SYMBOLS 


ACRONYM 

Description 

BC 

Boundary  Condition 

BVMC 

B-Spline  Augmented  Virtual  Motion  Camouflage 

CoV 

Calculus  of  Variations 

CPU 

Central  Process  Unit 

DC 

Direct  Collocation 

E.C. 

Equality  Constraints 

GUI 

Graphical  User  Interface 

I.E.C. 

Inequality  Constraints 

NLP 

Nonlinear  Programming 

LGL 

Legendre-Gauss-Lobatto 

LP 

Local  pursuit 

MATLAB 

Matrix  Laboratory 

MC 

Motion  Camouflage 

NURBS 

Non-uniform  Rational  B-Spline 

PI 

Performance  Index 

PMP 

Pontryagin’s  Minimum  Principle 

PCP 

Path  Control  Parameter 

RAM 

Random  Access  Memory 

SSA 

Space  Situational  Awareness 

VMC 

Virtual  Motion  Camouflage 
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